/*
 * robo_control.h
 *
 *  Created on: Apr 1, 2010
 *      Author: Aron
 *
 *  A mess... an attempt to implement a PID controller for a single motor..
 *
 *  ...should link them at some point
 */

//working, 50 hz

#ifndef ROBO_CONTROL_H_
#define ROBO_CONTROL_H_

# include "init_systems.h"
# include "time_tools.h"
# include "log.h"
# include "rrcon.h"
# include "xtask.h"
# include "cmath"

using namespace std;
using namespace kthrobot;

namespace Group5 {

class Robo_Control : public XTask {
public:
  Robo_Control(const string &name,int motor_number);
  void SetTargetSpeed(int speed);
  void Set_uP_mc(double up);

  double GetLastSpeed();
  double GetRelSpeed();
  void TaskCode();
  void Stop();
  static const int hz_ = 50;

  Motor& motor;

  virtual ~Robo_Control();

private:
  double getSpeed();
  int motor_number_;

  double old_target_speed_;
  double target_speed_;
  double setpoint_speed_;
  double actual_speed_;
  double speed_since_last_;

  double uP_mc_;
  double h_;
  double bi_;
  double ad_;
  double bd_;
  double a0_;
  double size_k_;
  //all constants are just guess work atm..
  //static gain

  static const double K_ = 36;
  // TD > Tt > Ti
  static const double TD_ = 0.05;
  static const double Tt_ = 0.4;
  static const double Ti_ = 50;

  static const double N_ = 10;

  //constant gain for the controller for both motors
  static const double K_mc_ = 5;

  //just a guess
  static const int max_speed_ = 12;
  static const double diam_ = 7.3;
  static const int deadband_size_ =320;

  static const int u_max_ = 1023;
  static const int u_min_ = -1023;

  static const double pos_max_speed_change_ = 5;
  static const double neg_max_speed_change_ = -5;
};

};
#endif /* ROBO_CONTROL_H_ */
